/****************************************************************
 * Fulguro
 * Copyright (C) 2004 Christophe Clienti
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2 of the License, or (at your option) any later version.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the
 * Free Software Foundation, Inc., 59 Temple Place - Suite 330,
 * Boston, MA 02111-1307, USA.
 ***************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#ifdef __SSE__
#include <mmintrin.h>
#include <xmmintrin.h>
#endif

#include <flgrCoreDispatch.h>

#include "flgrMeasureBaseSSE.h"


#define M64_SIZE sizeof(__m64)
#define M64_VEC_SIZE_UINT8  (M64_SIZE/sizeof(fgUINT8))
#define M64_VEC_SIZE_INT16  (M64_SIZE/sizeof(fgINT16))
#define M64_VEC_SIZE_UINT16  (M64_SIZE/sizeof(fgUINT16))
#define M64_VEC_SIZE_UINT32 (M64_SIZE/sizeof(fgUINT32))



void flgr1d_measure_min_max_spp1_SSE_fgUINT8(FLGR_Data1D *dat, FLGR_Vector *mini, FLGR_Vector *maxi) {
#ifdef __SSE__
  fgUINT8 *psrc = (fgUINT8*) dat->array;
  fgUINT8 array_min[M64_VEC_SIZE_UINT8];
  fgUINT8 array_max[M64_VEC_SIZE_UINT8];
  fgUINT8 _mini, _maxi;
  __m64 rmax,rmin,vec;
  int i, length = dat->length;
  int length_vecnorm = (length/M64_VEC_SIZE_UINT8)*M64_VEC_SIZE_UINT8;

  FLGR_DECLARE_FUNCTION;

  _mm_empty();

  rmin = *((__m64*) psrc);
  rmax = *((__m64*) psrc);

  for(i=M64_VEC_SIZE_UINT8 ; i<length_vecnorm ; i+=M64_VEC_SIZE_UINT8) {
    vec = *((__m64*) (psrc+i));
    rmin = _mm_min_pu8(rmin,vec);
    rmax = _mm_max_pu8(rmax,vec);
  }

  *((__m64*) array_min) = rmin;
  *((__m64*) array_max) = rmax;

  _mm_empty();

  _mini = array_min[0];
  _maxi = array_max[0];

  for(i=1 ; i<M64_VEC_SIZE_UINT8 ; i++) {
    _mini = FLGR_MIN(_mini,array_min[i]);
    _maxi = FLGR_MAX(_maxi,array_max[i]);
  }

  for(i=length_vecnorm ; i<length ; i++) {
    _mini = FLGR_MIN(psrc[i],_mini);
    _maxi = FLGR_MAX(psrc[i],_maxi);
  }

  *((fgUINT8*) mini->array) = _mini;
  *((fgUINT8*) maxi->array) = _maxi;
#endif
}


void flgr1d_measure_min_max_spp1_SSE_fgINT16(FLGR_Data1D *dat, FLGR_Vector *mini, FLGR_Vector *maxi) {
#ifdef __SSE__
  fgINT16 *psrc = (fgINT16*) dat->array;
  fgINT16 array_min[M64_VEC_SIZE_INT16];
  fgINT16 array_max[M64_VEC_SIZE_INT16];
  fgINT16 _mini, _maxi;
  __m64 rmax,rmin,vec;
  int i, length = dat->length;
  int length_vecnorm = (length/M64_VEC_SIZE_INT16)*M64_VEC_SIZE_INT16;

  FLGR_DECLARE_FUNCTION;

  _mm_empty();

  rmin = *((__m64*) psrc);
  rmax = *((__m64*) psrc);

  for(i=M64_VEC_SIZE_INT16 ; i<length_vecnorm ; i+=M64_VEC_SIZE_INT16) {
    vec = *((__m64*) (psrc+i));
    rmin = _mm_min_pi16(rmin,vec);
    rmax = _mm_max_pi16(rmax,vec);
  }

  *((__m64*) array_min) = rmin;
  *((__m64*) array_max) = rmax;

  _mm_empty();

  _mini = array_min[0];
  _maxi = array_max[0];

  for(i=1 ; i<M64_VEC_SIZE_INT16 ; i++) {
    _mini = FLGR_MIN(_mini,array_min[i]);
    _maxi = FLGR_MAX(_maxi,array_max[i]);
  }

  for(i=length_vecnorm ; i<length ; i++) {
    _mini = FLGR_MIN(psrc[i],_mini);
    _maxi = FLGR_MAX(psrc[i],_maxi);
  }

  *((fgINT16*) mini->array) = _mini;
  *((fgINT16*) maxi->array) = _maxi;
#endif
}


void flgr1d_measure_volume_u32_spp1_SSE_fgUINT8(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE__
  fgUINT32 array_vol[M64_VEC_SIZE_UINT32];
  fgUINT32 _volume;
  fgUINT8 *psrc = (fgUINT8*) dat->array;
  int i, length = dat->length;
  int length_vecnorm = (length/M64_VEC_SIZE_UINT8)*M64_VEC_SIZE_UINT8;

  __m64 rvol;
  __m64 v1, v2;
  __m64 v3;
  __m64 vec_zero;
 
  FLGR_DECLARE_FUNCTION;

  _mm_empty();

  v2 = *((__m64*) (psrc)); //just to not let v1 initialized
  vec_zero = _mm_xor_si64(v2,v2); //vec_zero = 0

  rvol = vec_zero;

  for(i=0 ; i<length_vecnorm ; i+=M64_VEC_SIZE_UINT8) {
    v2 = *((__m64*) (psrc+i));

    v1 = _mm_unpackhi_pi8(v2,vec_zero);
    v2 = _mm_unpacklo_pi8(v2,vec_zero);

    v3 = _mm_unpackhi_pi16(v1,vec_zero);
    v1 = _mm_unpacklo_pi16(v1,vec_zero);
    v1 = _mm_add_pi32(v1,v3);

    v3 = _mm_unpackhi_pi16(v2,vec_zero);
    v2 = _mm_unpacklo_pi16(v2,vec_zero);
    v2 = _mm_add_pi32(v2,v3);

    v1 = _mm_add_pi32(v1,v2);

    rvol = _mm_add_pi32(v1,rvol);
  }

  *((__m64*) array_vol) = rvol;

  _mm_empty();

  _volume = array_vol[0] + array_vol[1];

  for(i=length_vecnorm ; i<length ; i++) {
    _volume += (fgUINT32) psrc[i];
  }

  *((fgUINT32*) volume->array) = _volume;
#endif
}



void flgr1d_measure_volume_u32_spp1_SSE_fgUINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE__
  fgUINT32 array_vol[2];
  fgUINT32 _volume=0;
  fgUINT16 *psrc = (fgUINT16*) dat->array;
  __m64 *vec_psrc = (__m64*) dat->array;
  __m64 rvol;
  __m64 v1, v2;
  __m64 vec_zero;
  int i, nbvector = dat->length>>2;

  FLGR_DECLARE_FUNCTION;

  _mm_empty();

  v2 = *((__m64*) (psrc)); //just to not let v1 initialized
  vec_zero = _mm_xor_si64(v2,v2); //vec_zero = 0
  
  rvol = vec_zero;

  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];
    v1 = _mm_unpackhi_pi16(v2,vec_zero);
    v2 = _mm_unpacklo_pi16(v2,vec_zero);

    v1 = _mm_add_pi32(v1,v2);

    rvol = _mm_add_pi32(v1,rvol);
  }

  *((__m64*) array_vol) = rvol;

  _mm_empty();

  _volume = array_vol[0] + array_vol[1];

  for(i=nbvector<<2 ; i<dat->length ; i++) {
    _volume += (fgUINT32) psrc[i];
  }

  *((fgUINT32*) volume->array) = _volume;
#endif
}




void flgr1d_measure_volume_u32_spp1_SSE_fgINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE__
  fgUINT32 array_vol[2];
  fgUINT32 _volume=0;
  fgINT16 *psrc = (fgINT16*) dat->array;
  __m64 *vec_psrc = (__m64*) dat->array;
  __m64 rvol;
  __m64 v1, v2;
  __m64 vec_zero;
  int i, nbvector = dat->length>>2;

  FLGR_DECLARE_FUNCTION;

  _mm_empty();

  v2 = *((__m64*) (psrc)); //just to not let v1 initialized
  vec_zero = _mm_xor_si64(v2,v2); //vec_zero = 0
  
  rvol = vec_zero;

  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];
    v1 = _mm_unpackhi_pi16(v2,vec_zero);
    v2 = _mm_unpacklo_pi16(v2,vec_zero);

    v1 = _mm_add_pi32(v1,v2);

    rvol = _mm_add_pi32(v1,rvol);
  }

  *((__m64*) array_vol) = rvol;

  _mm_empty();

  _volume = array_vol[0] + array_vol[1];

  for(i=nbvector<<2 ; i<dat->length ; i++) {
    _volume += (fgUINT32) psrc[i];
  }

  *((fgUINT32*) volume->array) = _volume;
#endif
}


void flgr1d_measure_volume_s32_spp1_SSE_fgINT16(FLGR_Data1D *dat, FLGR_Vector *volume) {
#ifdef __SSE__
  fgINT32 array_vol[2];
  fgINT32 _volume=0;
  fgINT16 *psrc = (fgINT16*) dat->array;
  __m64 *vec_psrc = (__m64*) dat->array;
  __m64 rvol;
  __m64 v1, v2;
  int i, nbvector = dat->length>>2;

  FLGR_DECLARE_FUNCTION;

  _mm_empty();

  v2 = *((__m64*) (psrc)); //just to not let v1 initialized
  rvol = _mm_xor_si64(v2,v2); //vec_zero = 0

  for(i=0 ; i<nbvector ; i++) {
    v2 = vec_psrc[i];

    v1 = _mm_unpackhi_pi16(v2,v2); //convert int16
    v1 = _mm_srai_pi32( v1, 16 ); //        to int32

    v2 = _mm_unpacklo_pi16(v2,v2); //convert int16
    v2 = _mm_srai_pi32( v2, 16 ); //        to int32

    v1 = _mm_add_pi32(v1,v2);

    rvol = _mm_add_pi32(v1,rvol);
  }

  *((__m64*) array_vol) = rvol;

  _mm_empty();

  _volume = array_vol[0] + array_vol[1];

  for(i=nbvector<<2 ; i<dat->length ; i++) {
    _volume += (fgINT32) psrc[i];
  }

  *((fgINT32*) volume->array) = _volume;
#endif
}



